/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import Team102Lib.MessageLogger;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.commands.DriveTankWithXBox;

/**
 *
 * @author Admin
 */
public class DriveTrain extends Subsystem {
    
    SpeedController frontLeft;
    SpeedController rearLeft;
    SpeedController frontRight;
    SpeedController rearRight;
    RobotDrive drive;
    double preRightJoyY;
    double preLeftJoyY;
    double rightJoyY;
    double leftJoyY;


    public DriveTrain() {
        frontLeft = new Victor(RobotMap.victor1Port);
        frontRight = new Victor(RobotMap.victor2Port);
        rearLeft = new Victor(RobotMap.victor3Port);
        rearRight = new Talon(RobotMap.talon1Port);
        
        drive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
        drive.setSafetyEnabled(false);

        /*        drive.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, true);
        drive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);
        drive.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, true);
        drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);
*/
    }
    public void initDefaultCommand() {
         setDefaultCommand(new DriveTankWithXBox());
    }
    public void driveTankWithXBox(Joystick xBoxStick) {

        preRightJoyY = xBoxStick.getRawAxis(RobotMap.xBoxRightYAxis);
        preLeftJoyY = xBoxStick.getRawAxis(RobotMap.xBoxLeftYAxis);

        rightJoyY = RobotMap.stickDeadBand.Deaden(preRightJoyY);
        leftJoyY = RobotMap.stickDeadBand.Deaden(preLeftJoyY);

//        MessageLogger.LogMessage(-preLeftJoyY + "\t" + preRightJoyY + "\t" + -leftJoyY + "\t" + rightJoyY);
        drive.tankDrive(-leftJoyY, rightJoyY);
    }
    public void tankDrive(double leftWheels, double rightWheels)
    {
        drive.tankDrive(leftWheels, rightWheels);        
    }
}
